package edu.tfh.s2.ehkbot.steuerung;

import java.util.Vector;

import edu.tfh.s2.ehkbot.EHKBotMain;
import edu.tfh.s2.ehkbot.gerete.Fahrgestell;
import edu.tfh.s2.ehkbot.gerete.sensoren.IndoorGPS;
import edu.tfh.s2.ehkbot.simulation.RoutingGUI;


/**
 * Die Contol Klasse EHKBotNavigieren versammelt das Wissen um den Bot zu Navigieren.
 * @author s2zehn
 *
 */
public class EHKBotNavigieren {

	/**
	 * Wird geworfen wenn kein Weg gefunden wurde.
	 * @author s2zehn
	 *
	 */
	public class KeinWegGefunden extends Exception {
		private static final long serialVersionUID = -8536059854661555183L;
	}

	/**
	 * Navigiert zu den Ziel von der aktuellen Position.
	 * @param ziel Das Ziel.
	 * @throws KeinWegGefunden
	 */
	public void navigiereZumZiel(Position ziel) throws KeinWegGefunden {
		/*
		 * Aktuelle Position
		 */
		Position start = IndoorGPS.getInstance().getCurrentPosition();
		/*
		 * Weg errechnen lassen.
		 */
		Vector<Position> way = EHKBotMain.getInstance().getKarte().errechneWeg(start, ziel);
		if (way.isEmpty() || way.size() < 2)
			throw new KeinWegGefunden();
		if (EHKBotMain.isSimulation())
			RoutingGUI.getInstance().showRoute(way);

		/*
		 * Wegepunkte abfahren.
		 */
		Fahrgestell fahrgestell = EHKBotMain.getInstance().getEHKBot().getFahrgestell();
		for (int i = 0; i < way.size() - 2; ++i) {
			Position current = way.get(i);
			Position next = way.get(i + 1);
			Richtung winkel = current.richtung(next); //berchnet den winkel von hier zu später
			Richtung currentRichtung = IndoorGPS.getInstance().getRichtung();

			/*
			 * Wenn die Richtung nicht stimmt drehen.
			 */
			Richtung zuDrehen = winkel.sub(currentRichtung);
			fahrgestell.dreheBot(zuDrehen);

			long entfernung = current.abstand(next);
			fahrgestell.fahreBot(entfernung);
			
			/*
			 * In der RotingGUI anzeigen.
			 */
			if (EHKBotMain.isSimulation()) {
				Vector<Position> tmp = EHKBotMain.getInstance().getKarte().errechneWeg(
					IndoorGPS.getInstance().getCurrentPosition(),
					ziel);
				RoutingGUI.getInstance().showRoute(tmp);
			}

		}

	}
}